
#include "isense.h"
#include <stdio.h> 
#include <stdlib.h> 
#include <windows.h>

//==========================================================================================
const char *systemType( int Type ) 
{
    switch( Type ) 
    {
    case ISD_NONE:
        return "Unknown";
    case ISD_PRECISION_SERIES:
        return "IS Precision Series";
    case ISD_INTERTRAX_SERIES:
        return "InterTrax Series";
    }
    return "Unknown";
}


//==========================================================================================
const char *systemName( int Model ) 
{
    switch( Model ) 
    {
    case ISD_IS300:
        return "IS-300 Series";
    case ISD_IS600:
        return "IS-600 Series";
    case ISD_IS900:
        return "IS-900 Series";
    case ISD_INTERTRAX:
        return "InterTrax 30";
    case ISD_INTERTRAX_2:
        return "InterTrax2";
    case ISD_INTERTRAX_LS:
        return "InterTraxLS";
    case ISD_INTERTRAX_LC:
        return "InterTraxLC";
    case ISD_ICUBE2:
    case ISD_ICUBE2_PRO:
    case ISD_ICUBE2B_PRO:
        return "InertiaCube2";
    case ISD_ICUBE3:
        return "InertiaCube3";
    case ISD_IS1200:
        return "IS-1200 Series";
    }
    return "Unknown";
}


//==========================================================================================
//
//  Get and display tracker information
//
//==========================================================================================
void showTrackerStats( ISD_TRACKER_HANDLE handle, ISD_HARDWARE_INFO_TYPE *hwInfo )
{
    ISD_TRACKER_INFO_TYPE Tracker;
    ISD_STATION_INFO_TYPE Station;
    WORD i, numStations = 4;
    char buf[20];

    if( ISD_GetTrackerConfig( handle, &Tracker, TRUE ) )
    {
        printf( "\n\n********** InterSense Tracker Information ***********\n\n" );

        printf( "Type:     %s device on port %d\n", 
            systemType(Tracker.TrackerType), Tracker.Port );
        
        printf("Model:    %s\n",  
            hwInfo->Valid ?  hwInfo->ModelName : systemName(Tracker.TrackerModel) );

        switch( Tracker.TrackerModel ) 
        {
        case ISD_IS300:
        case ISD_IS1200:
            numStations = 4;
            break;
        case ISD_IS600:
        case ISD_IS900:
            numStations = ISD_MAX_STATIONS;
            break;
        default:
            numStations = 1;
            break;
        }
        
        printf( "\nStation\tTime\tState\tCube  Enhancement  Sensitivity  Compass  Prediction\n" );
        
        for(i = 1; i <= numStations; i++)
        {
            printf( "%d\t", i );
            
            if( ISD_GetStationConfig( handle, &Station, i, FALSE ))
            {
                sprintf(buf, "%d", Station.InertiaCube);
                
                printf( "%s\t%s\t%s\t   %u\t\t%u\t  %u\t  %u\n", 
                    Station.TimeStamped ? "ON" : "OFF", 
                    Station.State ? "ON" : "OFF", 
                    Station.InertiaCube == -1 ? "None" : buf, 
                    Station.Enhancement, 
                    Station.Sensitivity, 
                    Station.Compass, 
                    Station.Prediction );
            }
            else
            {
                printf("ISD_GetStationConfig failed\n");
                break;
            }
        }
        printf("\n");
    }
    else
    {
        printf("ISD_GetTrackerConfig failed\n");
    }
}


//==========================================================================================
//
//  Display Tracker data
//
//==========================================================================================
void showStationData( ISD_TRACKER_HANDLE             handle, 
                      ISD_TRACKER_INFO_TYPE         *Tracker,
                      ISD_STATION_INFO_TYPE         *Station,
                      ISD_STATION_DATA_TYPE         *data )
{
    // Get comm port statistics for display with tracker data 
    if( ISD_GetCommInfo( handle, Tracker ) )
    {
        printf( "%3.0fKbps %d R/s", 
            Tracker->KBitsPerSec, Tracker->RecordsPerSec );

        // display position only if system supports it 
        if( Tracker->TrackerModel == ISD_IS600 || 
            Tracker->TrackerModel == ISD_IS900 ||
            Tracker->TrackerModel == ISD_IS1200 )
        {
            printf( "%d %6.2f %6.2f %6.2f ", data->TrackingStatus,
                data->Position[0], data->Position[1], data->Position[2] );
        }

        // all products can return orientation 
        if( Station->AngleFormat == ISD_QUATERNION )
        {
            printf( "%5.2f %5.2f %5.2f %5.2f ",
                data->Quaternion[0], data->Quaternion[1], 
                data->Quaternion[2], data->Quaternion[3] );
        }
        else // Euler angles
        {
            printf( "%7.2f %7.2f %7.2f (%7.2f)",
                data->Euler[0], data->Euler[1], data->Euler[2], data->TimeStamp );
        }

        if( Station->GetAuxInputs ) 
        {
            printf("%d %d %d %d ", 
                (int) data->AuxInputs[0], 
                (int) data->AuxInputs[1], 
                (int) data->AuxInputs[2], 
                (int) data->AuxInputs[3]);
        }
        
        // if system is configured to read stylus or wand buttons 
        if( Station->GetInputs ) 
        {
            // Currently available products have at most 5 buttons,
            // stylus has 2, wand has 5
            printf("%d%d%d%d%d ", 
                (int) data->ButtonState[0], 
                (int) data->ButtonState[1], 
                (int) data->ButtonState[2], 
                (int) data->ButtonState[3], 
                (int) data->ButtonState[4]);

            printf( "%d %d ", data->AnalogData[0], data->AnalogData[1] ); 
        }

        printf("\r");
        fflush(0);
    }
}
